/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/*
*  \class	TOccupancyGrid
*  \author 	Jari Saarinen
*  \date 	Mon Jun 12 22:16:05 2006

*  \version: 	0.1  
*/

#include <stdio.h>
#include "TOccupancyGrid.h"


TOccupancyGrid::TOccupancyGrid(){
	initDefaults();
	mallocData();
}
 /**
 * Constructor
 * @p size_x the size of the grid in x-direction, in meters
 * @p size_y the size of the grid in y-direction, in meters
 * @p reso   Resolution. The size of one cell in meters
 * @p origin The position of the grid origin in the world frame of reference
	 **/
TOccupancyGrid::TOccupancyGrid(float size_x, 
															 float size_y, 
															 float reso, 
															 Grid::TCoord2D origin)
{
	initDefaults();
	if(reso<=0){
		fprintf(stderr,"TOccupancyGrid::TOccupancyGrid():Resolution must be grater than zero!\n");
		exit(1);
	}
	Nx = (int)(size_x/reso);
	Ny = (int)(size_y/reso);
	resolution = reso;
	world_pos = origin;
	information_sum = 0;
	mallocData();
}
TOccupancyGrid::~TOccupancyGrid(){
	if(data) free(data);
}
/**
* Get direct access to the data pointer
* The data is stored in one dimensional data pointer and ordered so that 
* index (i,j)=data[j*Ny+i]
* @return pointer to data;
*/
grid_t *TOccupancyGrid::getDataPtr(){
return data;
}
/**
* Returns the dimension of grid (number of cells in x and y direction)
* @p &x number of cells in x-axis
* @p &y number of cells in y-axis
*/
void TOccupancyGrid::getDim(int &x, int&y){
 x = Nx;
 y = Ny;
}

/**
* Returns the grid position in world coordinate frame
* @return the coordinates of the origin
*/
Grid::TCoord2D TOccupancyGrid::getWorldPos(){
	return world_pos;
}

/**
* returns the resolution = the number of meters that one cell represents 
*/
float TOccupancyGrid::getResolution(){
	return resolution;
}
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////

/**
* Transform a world coordinate position to 
* pixel coordinate position
* @p P a position in world coordinates
* @return the P in pixel coordinates
*/
Grid::TPixel TOccupancyGrid::toPixel(Grid::TCoord2D P){
	float x,y;
	Grid::TPixel pix;
	///Relative metric coordinates
	x = P.x - world_pos.x;
	y = P.y - world_pos.y;
	
	/// Into pixels
	x /= resolution;
	y /= resolution;
	
	/// The origin is in the centre of grid 
	x += Nx/2; 
	y = Ny/2 - y;  
	pix.x = (int)x;
	pix.y = (int)y;
	return pix;
}


/**
* Transform pixel coordinate to world coordinate
* @p P a pixel position
* @return the position of the pixel in world coordinates
*/
Grid::TCoord2D TOccupancyGrid::toCoord(Grid::TPixel P){
	Grid::TCoord2D coord;
	int x,y; 

	x = P.x - Nx/2;
	y = -P.y + Ny/2; 
	
	coord.x = ((float) x) * resolution + world_pos.x;
	coord.y = ((float) y) * resolution + world_pos.y;		
			
	return coord;
}
/**
* Changes an measurement taken in pose (x,y,a) to TCoord2D
* @p x,y,a the robot pose in the world
* @p r,ra the measurement
**/
Grid::TCoord2D TOccupancyGrid::toCoord(float x, float y, float a, float r, float ra){
	Grid::TCoord2D coord;
	
	coord.x = x + r * cos(a+ra);
	coord.y = y + r * sin(a+ra);
	
	return coord;
}
/**
* Set value to pixel position P
* @p P The position in grid_t
* @p value The value that you want to set
* @return true in success, else false
*/
bool TOccupancyGrid::setValue(Grid::TPixel P,grid_t value){
	if(isValid(P)){
		data[P.y*Nx + P.x]=value;
		return true;
	}
	return false;
}

/**
* Sets value to grid in world position P
* @p P a position in world
* @p value the value that you want to set
* @return true in success, else false
*/
bool TOccupancyGrid::setValue(Grid::TCoord2D P, grid_t value){
	Grid::TPixel pix;
	pix = toPixel(P);
	//fprintf(stderr,"Setting to (%.2f%.2f)=(%d,%d)\n",P.x,P.y,pix.x,pix.y);
	if(isValid(pix)){
		data[pix.y*Nx + pix.x]=value;
		return true;
	}
	return false;
}

/**
* get value of the grid from world position P
* @p P a position where you want to take a value
* @p &value the value is copied here
* @return true in success, else false 
**/
bool TOccupancyGrid::getValue(Grid::TCoord2D P, grid_t &value){
	Grid::TPixel pix;
	pix = toPixel(P);
	if(isValid(pix)){
		value = data[pix.y*Nx + pix.x];
		return true;
	}
	//fprintf(stderr,"pix=(%d,%d) N=(%d,%d) ",pix.x,pix.y,Nx,Ny );
	return false;
}

/**
* Gets a value from pixel position P
* @p P the pixel coordinate that you want to retreive the data from
* @p &value The value is copied here
* @return true in success, else false
*/
bool TOccupancyGrid::getValue(Grid::TPixel P, grid_t &value){
	if(isValid(P)){
		value = data[P.y*Nx + P.x];
		return true;
	}
	return false;
}

/**
* Returns index of given pixel
* @p P the pixel coordinate that you want to retreive the data from
* @p &value The value is copied here
* @return true in success, else false
*/
bool TOccupancyGrid::getIndex(Grid::TPixel P, int &value){
	if(isValid(P)){
		value = P.y*Nx + P.x;
		return true;
	}
	return false;
}

/**
* Returns corresponding TPixel of a given grid index
* @p index the grid index you want to match
* @p &P The point is copied here
*/
void TOccupancyGrid::getPoint(int index, Grid::TPixel &P){
	P.y = index%Nx;
	P.x = index%Ny;
}



/**
 * Sets the whole grid to given value
 * @p value The wanted grid value
 */
void TOccupancyGrid::setGridToValue(grid_t value){
	for(int i=0;i<Nx*Ny;i++) data[i]=value;
}
	
	
/**
 * Updates the Observation probability @P to the grid position @Pos.
 * @param Pos the coordinate which probability will be updated
 * @param P The observation probability
 * @return true
 */
bool TOccupancyGrid::updateProbability(Grid::TCoord2D Pos, float P){
	float Pold;
	float Pnew;
	grid_t p;
	
	getValue(Pos,p); 				///< To float
	if(p == 0.5) { ///FIXME: Maybe should use PROB_VALUE_MAX/2 but might be difficult
		information_sum++;
	}
	Pold = ((float)p)/(float)PROB_VALUE_MAX; 		///< Old probability
	
	Pnew = ( Pold*P) / ( Pold*P + (1.0f-Pold)*(1.0f-P)); 	///< Updated probability
	p = (grid_t) (PROB_VALUE_MAX * Pnew); 		///< Back to char
	
	setValue(Pos,p);
	return true;
	
}

/**
 * Updates the Observation probability @P to the grid position @Pos.
 * @param Pos the coordinate which probability will be updated
 * @param P The observation probability
 * @return true
 */
bool TOccupancyGrid::updateProbability(Grid::TPixel Pos, float P){
	float Pold;
	float Pnew;
	grid_t p;
	
	getValue(Pos,p); 				///< To float
	if(p == 0.5) { ///FIXME: Maybe should use PROB_VALUE_MAX/2 but might be difficult
		information_sum++;
	}
	Pold = ((float)p)/(float)PROB_VALUE_MAX; 		///< Old probability
	Pnew = ( Pold*P) / ( Pold*P + (1.0f-Pold)*(1.0f-P)); 	///< Updated probability
	p = (grid_t) (PROB_VALUE_MAX * Pnew); 		///< Back to char
	//fprintf(stderr,"%.3f %.3f\n",PROB_VALUE_MAX,p);
	setValue(Pos,p);
	return true;
	
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
/**
* Sets a line to the grid. The line is given as start and end coordinates
* The value that the line is set is given with @value input argument
* @p x1 x-coordinate of the start point
* @p y1 y-coordinate of the start point
* @p x2 x-coordinate of the end point
* @p y2 y-coordinate of the end point
* @p value The value that the line is set to
* @return true in success, else false
**/
bool TOccupancyGrid::setLine(float x1,float y1,float x2,float y2, grid_t value){
	float alpha=atan2(y2-y1,x2-x1);
	float r = sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));
	
	return setLine(Grid::TCoord2D(x1,y1),r,alpha,value);
}

/**
* Sets a line to grid. The line is given as start point and angle, length pair.
* @p pos the start point of the line
* @p r The length of the line
* @p angle angle of the line
* @p value The value the grid cells under the line is set to
* @p checkRigid Set true if you have rigid structures in map
* @p r_val the value of the rigid cell
* @return true in success, else false
**/
bool TOccupancyGrid::setLine(Grid::TCoord2D pos, 
														 float r, 
														 float angle, 
														 grid_t value,
														 bool checkRigid, 
														 grid_t r_val)
{
	float dl=resolution;
	float length=0;
	float cosa=cos(angle);
	float sina=sin(angle);
	grid_t v=0;
	
	while(length<r){
		if(checkRigid){
			getValue(pos,v);
			if(v!=r_val){
				setValue(pos,value);
			}
		}else{
			setValue(pos,value);
		}
		
		pos.x += dl*cosa;
		pos.y += dl*sina;
		length+=dl;
	}
	return true;
}

/**
 * Sets a line to grid. The line is given as start point and angle, length pair.
 * The grid cell is set to value @value only if it was previously in value @unknown
 * 
 * @p pos the start point of the line
 * @p r The length of the line
 * @p angle angle of the line
 * @p value The value the grid cells under the line is set to
 * @p unknown The value that the grid need to be before it is set to @value
 * @return true in success, else false
 **/
bool TOccupancyGrid::setLine(Grid::TCoord2D pos, 
														 float r, 
														 float angle, 
														 grid_t value, grid_t unknown)
{
		float dl=resolution;
		float length=0;
		float cosa=cos(angle);
		float sina=sin(angle);
		grid_t v=0;
	
		while(length<r){
				getValue(pos,v);
				if(v==unknown){
						setValue(pos,value);
				}
				pos.x += dl*cosa;
				pos.y += dl*sina;
				length+=dl;
		}
		return true;
}


/**
* updates probability on a line to grid. 
* The line is given as start point and angle, length pair.
* @p pos the start point of the line
* @p r The length of the line
* @p angle angle of the line
* @p P The observation probability
* @return true in success, else false
**/
bool TOccupancyGrid::setBayesLine(Grid::TCoord2D pos, float r, float angle, float P){
	float dl=resolution;
	float length=0;
	float cosa=cos(angle);
	float sina=sin(angle);
	/*
	float dy = sin(pose[2]);
	float dx = cos(pose[2]);
	float r=0;
	int i=0;
	uint x,y;
	while(r<length){
	x = pix + i*dx;
	y = piy + i*dy;
	i++;
	r+=resolution;
	*/
	Grid::TPixel pix = toPixel(pos);
	Grid::TPixel p;
	int ind=0;
	while(length<r){
		p.x = pix.x + ind*cosa;
		p.y = pix.y - ind*sina;
		
		updateProbability(p, P);
		//updateProbability(pos, P);
		//pos.x += dl*cosa;
		//pos.y += dl*sina;
		length+=resolution;
		ind++;
	}
	return true;	
}
	

/**
* Sets a beam into the grid. By beam I mean a range measurement that has 
* a limited beam width (not only a straigth line). e.g. SICK laser scanner typically has
* beam width of 0.5degrees, the outdoor version has about 0.75degrees.  
* @p pos the start position of the beam
* @p r the length of the beam
* @p angle the angle of the beam centre
* @p beamWidth How wide is the beam
* @p value to which value the grid is set to.
* @return true in success, else false
*/
bool TOccupancyGrid::setBeam(Grid::TCoord2D pos, float r, float angle,float beamWidth, grid_t value){
	float width;
	float a = angle-beamWidth/2.0; ///<Beam start angle
	float da;
	//This is approximately how wide the beam is in the end of beam [m]
	width = 2.0 * sin(beamWidth/2.0)*r;
	
	float numA = width/resolution; ///< This is how many lines we need
	da = beamWidth/numA; ///< Delta angle
	
	///Draws the beam as lines. This is not very efficient algorithm.
	while(a<=(angle+beamWidth/2.0)){
		setLine(pos,r,a,value);
		a+=da;
	}
	
	return true;
}

/**
* Set a circle to map 
* @p pos the position of the centre of the circle
* @p r The radius of the circle
* @p value To which value the grid is set to. 
* @return true in success, else false
*/
bool TOccupancyGrid::setCircle(Grid::TCoord2D pos, float r, grid_t value){
	Grid::TPixel pix = toPixel(pos);
	int radius = int (r/resolution);
	
		// set pixels (i,j) in image corresponding to given circle parameters
	int i, j, diff2, diff;
	for (i = pix.x - radius; i <= pix.x + radius; ++i) {
		diff2 = (radius * radius) - ((i - pix.x) * (i - pix.x));
		diff = (int) sqrt(((double) diff2));
		j = pix.y - diff;
		setValue(Grid::TPixel(i,j),value);
		j = pix.y + diff;
		setValue(Grid::TPixel(i,j),value);
	}
	
	return true;
}


/**
 * Sets a an arc into the grid. The arc is defined the same way as beam, but
 * now only the only the values at the end of @r are set (i.e arc) 
 * @p pos the start position of the beam
 * @p r the length of the beam
 * @p angle the angle of the beam centre
 * @p beamWidth How wide is the beam
 * @p value to which value the grid is set to.
 * @return true in success, else false
 */
bool TOccupancyGrid::setArc(Grid::TCoord2D pos, 
														float r, 
														float angle,
														float beamWidth, 
														grid_t value)
{
	float width;
	float a = angle-beamWidth/2.0; ///<Beam start angle
	float da;
	float x,y;
	
	//This is approximately how wide the beam is in the end of beam [m]
	width = 2.0 * sin(beamWidth/2.0)*r;
	
	float numA = width/resolution; ///< This is how many lines we need
	da = beamWidth/numA; ///< Delta angle
	
	///Draws the beam as lines. This is not very efficient algorithm.
	while(a<=(angle+beamWidth/2.0)){
		x = pos.x + r*cos(a);
		y = pos.y + r*sin(a);
		setValue(Grid::TCoord2D(x,y),value);
		a+=da;
	}

return true;
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
/**
* This method will set the scan end points to grid 
* The (x,y,a) is the robot/sensor Pose in the world
* The s contains the scan. For each point there is a (r,a)-pair which defines the 
* range and angle with respect to robot of each measurement.
* @p x x-coordinate of the robot
* @p y y-coordinate of the robot
* @p a heading of the robot
* @p s The scan
* @p value The value the scan points are set.
* @return true in success, else false
*/
bool TOccupancyGrid::setScanPoints(float x, float y ,float a, Grid::TScan &s, grid_t value){
	for(int i=0;i<s.N;i++){
			if(s.r[i]>0) setValue(toCoord(x,y,a,s.r[i],s.a[i]),value);
	}
	return true;
}	

/**
 * This method will set the scan end points to grid 
 * The grid is set to @value only if the previous value was @unknown
 *
 * The (x,y,a) is the robot/sensor Pose in the world
 * The s contains the scan. For each point there is a (r,a)-pair which defines the 
 * range and angle with respect to robot of each measurement.
 * @p x x-coordinate of the robot
 * @p y y-coordinate of the robot
 * @p a heading of the robot
 * @p s The scan
 * @p value The value the scan points are set.
 * @p unknown The value that the grid must be in order for the value to be set.
 * @return true in success, else false
 */
bool TOccupancyGrid::setScanPoints(float x, float y ,float a, Grid::TScan &s, grid_t value, grid_t unknown){
		grid_t v;
		for(int i=0;i<s.N;i++){
				if(s.r[i]>0){
						Grid::TCoord2D p = toCoord(x,y,a,s.r[i],s.a[i]);
						getValue(p,v);
						if(v == unknown)
								setValue(p,value);
				}
		}
		return true;
}	


/**
* This method will fill the scan to grid. It means that the endpoints are set as 
* occupied (o_val) and the points that don't contain the obstacles are set as 
* empty (e_val)
*
* The (x,y,a) is the robot/sensor Pose in the world
* The s contains the scan. For each point there is a (r,a)-pair which defines the 
* range and angle with respect to robot of each measurement.
* @p x x-coordinate of the robot
* @p y y-coordinate of the robot
* @p a heading of the robot
* @p s The scan
* @p e_val The value the scan hit points are set = occupied
* @p o_val The value for empty cell
* @return true in success, else false
*/
bool TOccupancyGrid::setScanFillSimple(
																			float x, 
																			float y ,
																			float a, 
																			Grid::TScan &s, 
																			grid_t e_val,
																			grid_t o_val)
{
	for(int i=0;i<s.N;i++){
		if(s.r[i]>0)
				setLine(Grid::TCoord2D(x,y),s.r[i]-resolution,s.a[i]+a,e_val);
	}
	setScanPoints(x,y,a,s,o_val);
	return true;
}	

/**
 * This method will fill the scan to grid. It means that the endpoints are set as 
 * occupied (o_val) and the points that don't contain the obstacles are set as 
 * empty (e_val)
 * 
 * Additionally this method checks if the cell is in the value @unknown. If not, 
 * it is not updated.  
 *
 * The (x,y,a) is the robot/sensor Pose in the world
 * The s contains the scan. For each point there is a (r,a)-pair which defines the 
 * range and angle with respect to robot of each measurement.
 * @p x x-coordinate of the robot
 * @p y y-coordinate of the robot
 * @p a heading of the robot
 * @p s The scan
 * @p e_val The value the scan hit points are set = occupied
 * @p o_val The value for empty cell
 * @p u_val Value of unknown cell
 * @return true in success, else false
 */
bool TOccupancyGrid::setScanFillSimple(float x, 
												 float y ,
												 float a, 
												 Grid::TScan &s, 
												 grid_t e_val,
												 grid_t o_val,
												 grid_t u_val)
{
		for(int i=0;i<s.N;i++){
				if(s.r[i]>0)
						setLine(Grid::TCoord2D(x,y),s.r[i]-resolution,s.a[i]+a,e_val,u_val);
		}
		setScanPoints(x,y,a,s,o_val,u_val);
		return true;
}


/**
* This method is the same as setScanFillSimple(), but instead of simply
* setting the grid cells into some value, the cell probabilities are updated
* with bayes formula. 
* This method uses define PROB_VALUE_MAX. This value can be used if 
* you have e.g. grid_t set to char (use PROB_VALUE_MAX 255)
* @p x x-coordinate of the robot
* @p y y-coordinate of the robot
* @p a heading of the robot
* @p s The scan
* @p Pe empty measurement probability
* @p Po occupied probability 
* @return true in success, else false
**/
bool TOccupancyGrid::setScanFillProb(float x, 
																		 float y ,
																		 float a, 
																		 Grid::TScan &s,
																		 float Pe,
																		 float Po)
{
	for(int i=0;i<s.N;i++){
		setBayesLine(Grid::TCoord2D(x,y),s.r[i]-resolution,s.a[i]+a,Pe);
		updateProbability(toCoord(x,y,a,s.r[i],s.a[i]),Po);
	}	
	return true;
}	
/**
* This is the same method as setScanFillSimple(), but this method will not changed
* the rigid structures in the map i.e. if the cell is in the r_val, then it will not beam
* updated, no matter what the measurement says.
*
* The (x,y,a) is the robot/sensor Pose in the world
* The s contains the scan. For each point there is a (r,a)-pair which defines the 
* range and angle with respect to robot of each measurement.
* @p x x-coordinate of the robot
* @p y y-coordinate of the robot
* @p a heading of the robot
* @p s The scan
* @p e_val The value the scan hit points are set = occupied
* @p o_val The value for empty cell
* @p r_val The value for the rigid obstacle
* @return true in success, else false
*/
bool TOccupancyGrid::setScanFillWithRigidWalls(float x, 
															float y ,
															float a, 
															Grid::TScan &s, 
															grid_t e_val,
															grid_t o_val,
															grid_t r_val)
{
	grid_t v=0;
	
	for(int i=0;i<s.N;i++){
		setLine(Grid::TCoord2D(x,y),s.r[i]-resolution,s.a[i]+a,e_val,true,r_val);
		
		getValue(toCoord(x,y,a,s.r[i],s.a[i]),v);
		if(v!=r_val){
			setValue(toCoord(x,y,a,s.r[i],s.a[i]),o_val);
		}
	}
	
	return true;
}	


///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////


/**
* Copies the grid data to RGB pointer
* The value of cell is divided with @scale
* The method fills the RGB as grey scale image 
* * The rgb pointer must be reserved. Size is expected to be 3*Nx*Ny
* @p *rgb pointer to RGB data
* @p scale divider for the grid data
* @return true
*/
bool TOccupancyGrid::fillRGB(unsigned char *rgb, float scale){
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			rgb[3*Nx*j+3*i] = (unsigned char) (data[j*Ny+i]/scale);
			rgb[3*Nx*j+3*i+1] = (unsigned char) (data[j*Ny+i]/scale);
			rgb[3*Nx*j+3*i+2] = (unsigned char) (data[j*Ny+i]/scale);
		}
	}
	return true;
}
/**
	* Copies the grid data to RGB pointer
	* The method uses empty, occupied and rigid values to 
	* set R=rigid,G=free and B=occupied values
	* If the value is none of the above, it is set to black
	* The rgb pointer must be reserved. Size is expected to be 3*Nx*Ny			
	* 
	* @p *rgb The RGB data pointer
	* @p e_val Value for empty=free cells
	* @p o_val Value for occupied cells
	* @p r_val Value for Rigid cells
	* @return true
	*/
bool TOccupancyGrid::fillRGB(unsigned char *rgb, grid_t e_val, grid_t o_val, grid_t r_val){
	grid_t v=0;
	unsigned char r,g,b;
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			getValue(Grid::TPixel(i,j),v);
			if(v==e_val){
				r = 0; g=255; b = 0;
			}else if(v==o_val){
				r = 0; g=0; b = 255;
			}
			else if(v==r_val){
				r = 255; g=0; b = 0;
			}else{
				r = 0; g=0; b = 0;
			}
			rgb[3*Nx*j+3*i] = r;
			rgb[3*Nx*j+3*i+1] = g;
			rgb[3*Nx*j+3*i+2] = b;
		}
	}
	return true;
}


///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
/**
* Returns a line taken from position (x,y) to direction a
* The line ends when cell value in direction a is equal or grater than
* o_val. Method returns the metric distance.
* @p x world coordinate x
* @p y world coordinate y
* @p a direction of line
* @return the range. -1 is returned if no hit.
**/
float TOccupancyGrid::getLine(float x,float y, float a,grid_t o_val){
	float dl=resolution;
	float length=0;
	float cosa=cos(a);
	float sina=sin(a);
	Grid::TCoord2D pos(x,y);
	grid_t v;
	while(1){
		
		if(!getValue(pos,v)){
			//fprintf(stderr,"Error l:%.2f Pos=(%.2f, %.2f)\n", length,pos.x,pos.y);
			return -1;
		}
		if(v>=o_val){
			return length;
		}
		pos.x += dl*cosa;
		pos.y += dl*sina;
		length+=dl;
		
		//fprintf(stderr,"%.2f ", length);
	}
	return -1;
}

/**
 * Returns a line taken from position (x,y) to direction a
 * The line ends when cell value in direction a is equal or grater than
 * o_val. Method returns the metric distance.
 * @p x world coordinate x
 * @p y world coordinate y
 * @p a direction of line
 * @p o_val threshold
 * @p *dt disance transformation matrix 
 * @return the range. -1 is returned if no hit.
 **/
float TOccupancyGrid::getLine(float x,float y, float a,grid_t o_val, float *dt){
	float dl=resolution;
	float length=0;
	float cosa=cos(a);
	float sina=sin(a);
	Grid::TCoord2D pos(x,y);
	grid_t v;
	Grid::TPixel pix;
	while(1){
		pix = toPixel(pos);
		if(!getValue(pix,v)){
			//fprintf(stderr,"Error l:%.2f Pos=(%.2f, %.2f)\n", length,pos.x,pos.y);
			return -1;
		}
		if(v>=o_val){
			return length;
		}
		dl = dt[pix.y*Nx+pix.x];
		pos.x += dl*cosa;
		pos.y += dl*sina;
		if(dl<resolution) dl = resolution;
		length+=dl;
		if(length > 80) return -1;
		//fprintf(stderr,"%.2f ", length);
	}
	return -1;
}



/**
	* returns a scan taken from (x,y). The first measurement is taken from angle @a 
	* @n is the number of measurements taken and @da is the angle difference between
	* measurements. The result is stored to @s. If the s.N != n then it is reallocated.
	* The o_val is used to check if the cell has obstacle
	* @p x world coordinate x
	* @p y world coordinate y
	* @p a direction of the first measurement
	* @p n number of measurements taken
	* @p da the angle difference between the measurements 
	* @p o_val The threshold for checking if the cell has obstacle
	* @p s The scan s.r[] will be -1 if no measurement can be taken 
	* @return true
	**/
bool TOccupancyGrid::getScan(float x,
														 float y, 
														 float a, 
														 int n, 
														 float da, 
														 grid_t o_val, 
														 Grid::TScan &s)
{
	float angle=a;
	float ang = 0;
	if(s.N != n){
		s.alloc(n);
	}
	
	for(int i = 0;i<n;i++){
		s.a[i] = ang;
		s.r[i] = getLine(x,y,angle,o_val);
		angle += da;
		ang+=da;
	}
	return true;
}

/**
* Makes a distance transform for the grid. The algorithm uses binary image, so 
* th is used to define if the cell is occupied or not
* @p *out Nx*Ny sized pointer that the distances are stored into 
* @p th Threshold for segmentation
* @return maximum distance
**/
float TOccupancyGrid::distanceTransform(float *out, grid_t th){
	image<uchar> *input = new image<uchar>(Nx,Ny);
	float maxd=0;
	int indx,indy;
	
	for(int y=0;y<Nx;y++){
		for(int x=0;x<Nx;x++){
			if(data[Nx*y+x]<th) imRef(input,x,y) = 0;
			else imRef(input,x,y) = 1;
		}
	}
	image<float> *output = dt(input);
	for(int y=0;y<Nx;y++){
		for(int x=0;x<Nx;x++){
			out[y*Nx+x] = sqrt(imRef(output,x,y))*resolution;
			if(out[y*Ny+x] > maxd){
				maxd = out[y*Ny+x];
				indx = x;
				indy = y;
			}

		}
	}
	delete input;
	delete output;
					
	return maxd;
}	 

/**
* This is the same method as above one, but it uses a distance transformation
* matrix to make the calculation more efficient
* returns a scan taken from (x,y). The first measurement is taken from angle @a 
* @n is the number of measurements taken and @da is the angle difference between
* measurements. The result is stored to @s. If the s.N != n then it is reallocated.
* The o_val is used to check if the cell has obstacle
* 
* @p x world coordinate x
* @p y world coordinate y
* @p a direction of the first measurement
* @p n number of measurements taken
* @p da the angle difference between the measurements 
* @p o_val The threshold for checking if the cell has obstacle
* @p *dt distance transform matrix calculated with distanceTransform()
* @p s The scan s.r[] will be -1 if no measurement can be taken 
* @return true
**/
bool TOccupancyGrid::getScan(float x,
							 float y, 
							 float a, 
							 int n, 
							 float da, 
							 grid_t o_val,
							 float *dt, 
							 Grid::TScan &s)
{
	float angle=a;
	float ang=0;
	if(s.N != n){
		s.alloc(n);
	}
	
	for(int i = 0;i<n;i++){
		s.a[i] = ang;
		s.r[i] = getLine(x,y,angle,o_val,dt);
		angle += da;
		ang+=da;
	}
	
	return true;
}


/**
* Get submap (copy) from the grid.
* The centre of the submap is @p centre. The left upper corner is centre.x-Nx/2,centre.y-Ny/2
* @param *submap The submap is copied here. Must be reserved size (nx*ny)
* @param centre The "center" pixel around which the submap is taken from
* @param nx size of submap in x-direction
* @param ny size of the submap in y-dir.
*/
bool TOccupancyGrid::getSubmap(float *submap, Grid::TPixel centre, int nx, int ny){
	int row_begin = centre.x - nx/2;
	int col_begin = centre.y - ny/2;
	int gridx,gridy;
	
	if(nx<0 || ny<0||nx>Nx||ny>Ny||submap == NULL|| !isValid(centre)) return false;
	
	for(int j=0;j<ny;j++){
		for(int i=0;i<nx;i++){
			gridx =(row_begin+i);
			gridy= (col_begin+j);
			if(isValid(Grid::TPixel(gridx,gridy))){
				submap[nx*j+i]=data[gridy*Nx+gridx];
			}else submap[nx*j+i] = 0.5;
		}	
	}
	return true;
}

/**
 * Set submap (copy) to the grid.
 * The centre of the submap is @p centre. The left upper corner is centre.x-Nx/2,centre.y-Ny/2
 * @param *submap The submap is copied here. Must be reserved size (nx*ny)
 * @param centre The "center" pixel around which the submap is taken from
 * @param nx size of submap in x-direction
 * @param ny size of the submap in y-dir.
 */
bool TOccupancyGrid::setSubmap(float *submap, Grid::TPixel centre, int nx, int ny){
	int row_begin = centre.x - nx/2;
	int col_begin = centre.y - ny/2;
	int gridx,gridy;
	
	if(nx<0 || ny<0||nx>Nx||ny>Ny||submap == NULL|| !isValid(centre)) return false;
	
	for(int j=0;j<ny;j++){
		for(int i=0;i<nx;i++){
			gridx =(row_begin+i);
			gridy= (col_begin+j);
			if(isValid(Grid::TPixel(gridx,gridy))){
				data[gridy*Nx+gridx]=submap[nx*j+i];
			}	
		}	
	}
	return true;
}




/**
* Returns the minimum of the surroundig of pixel in pos (x,y)
* @p x x index in grid
* @p y y index in grid
* @return the minimum value around (x,y)
*/
grid_t TOccupancyGrid::getMin(int x, int y){
  grid_t min_val=10000;
	int cnt = 0;
	int num;

	num = 1;
	  /// X-coordinates
	for(int i=x-num;i<x + num ; i++ ){
		// Y-coordinates
		for(int j=y -num ;j<y + num;j++){
			
			if(i>= 0 && j>=0 && i<Nx && j<=Ny){
				if(i == x && j==y  ){
				}
				else{
					grid_t val;
					if(!(x<0 || x>=Nx || y<0 || y>=Ny)){
						val = data[j*Ny+i];
						if(val<min_val) min_val = val;
						cnt++;
					}
				}
			}
		}
	}/// For x
	
	if(cnt == 0) return data[y*Ny+x];
	
	return min_val;
}
/**
 * Computes a gradient of the grid. 
 * The method computes the gradient as [value-(minimum of neighbour cells)]/resolution 
 * @param *out Output pointer. If null the memory is reserved
 * @return maximum grid value
 **/
float TOccupancyGrid::gradient(grid_t *out){
  float maxv=-10000;
	if(out==NULL) out = (grid_t *) malloc(Nx*Ny*sizeof(grid_t));
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			out[j*Ny+i] = (data[j*Ny+i] - getMin(i,j))/resolution;
			if(out[j*Ny+i] > maxv) maxv=out[j*Ny+i];
		}///Nx
	}///j == Ny
	
	return maxv;
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
								
/**
 * Saves the data to file
 * @return true, if success, else false
 */
bool TOccupancyGrid::save(const char *fname){
	FILE *f = fopen(fname,"wt");
	if(f==NULL){
		fprintf(stderr,"Unable to open file '%s'\n",fname);
		return false;
	}
	fprintf(f,"%d %d %f %f %f\n",Nx,Ny,resolution,world_pos.x,world_pos.y);
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			fprintf(f,"%.3f ",data[j*Nx+i]);
		}
		fprintf(f,"\n");
	}
	fclose(f); 
	return true;
}
	
/**
	* Loads the data from the file 
	*/
bool TOccupancyGrid::load(const char *fname){
	FILE *f = fopen(fname,"rt");
	if(f==NULL){
		fprintf(stderr,"Unable to open file '%s'\n",fname);
		return false;
	}
	fscanf(f,"%d %d %f %f %f\n",&Nx,&Ny,&resolution,&world_pos.x,&world_pos.y);
	mallocData();
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			fscanf(f,"%f ",&data[j*Nx+i]);
		}
		fscanf(f,"\n");
	}
	fclose(f);
	return true;
}

/**
	* Loads the data in Ground Model format
	**/
bool TOccupancyGrid::loadGModel(const char *fname){
	FILE *f = fopen(fname,"rt");
	if(f==NULL){
		fprintf(	stderr,"Unable to open file '%s'\n",fname);
		return false;
	}
	int pix_origin_x,pix_origin_y;
	float sx,sy;
	fscanf(f,"size_x:%f\n",&sx);
	fscanf(f,"size_y:%f\n",&sy);
	fscanf(f,"reso:%f\n",&resolution);
	fscanf(f,"orig_x:%d\n",&pix_origin_x);
	fscanf(f,"orig_y:%d\n",&pix_origin_y);	
	world_pos.x=0;
	world_pos.y=0;
	Nx = (int) (sx/resolution)+1;
	Ny = (int) (sx/resolution)+1;
	mallocData();
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			fscanf(f,"%f ",&data[j*Nx+i]);
		}
		fscanf(f,"\n");
	}
	
	fprintf(stderr,"Read '%s' size=(%d,%d), resolution=%.2f\n",fname,Nx,Ny,resolution);
	fclose(f);
	return true;
}

/**
 * Loads a line map (in format '%f %f %f %f\n')
 * and puts it to appropriate sized map
 * @p *fname the filename
 * @p scale  the scale for the line map (the number from the file is multiplied with this)
 * @p lineval The value that the line is set to
 * @p rescale set false if you don't want that the map is rescaled when the map is read.
 * @p reset Set true if you want to reset the grid to value @reset_val
 * @p reset val the value to reset the grid to
 * @return true if success.
 **/
bool TOccupancyGrid::loadLineMap(const char *fname, float scale,grid_t lineval,bool rescale,bool reset,grid_t reset_val){
	std::vector<Grid::TLine> lines;
	FILE *f=fopen(fname,"rt");
	if (f==NULL){
		fprintf(stderr,"File '%s'is NULL\n",fname);
		return false;
	}
	
	Grid::TLine line;
	float max=-1000;
	while (fscanf(f,"%f %f %f %f",&line.x1,&line.y1,&line.x2,&line.y2)==4){
		lines.push_back(line);
		if(fabs(line.x1)>max) max=fabs(line.x1);
		if(fabs(line.x2)>max) max=fabs(line.x2);
		if(fabs(line.y1)>max) max=fabs(line.y1);
		if(fabs(line.y2)>max) max=fabs(line.y2);
	}
	fclose(f);
	
	if(rescale){	
	 Nx = 2.0*max/resolution;
	 Ny = 2.0*max/resolution;
	 mallocData();
	}
	
	if(reset){
	 setGridToValue(reset_val);
	}
		
	fprintf(stderr,"Reset='%d', reset_val=%f scale=%f, lineval=%f\n",reset,reset_val,scale,lineval);
			
	for(int i=0;i<lines.size();i++){
		setLine(lines[i].x1,lines[i].y1,lines[i].x2, lines[i].y2, lineval);
	}
	
	return true;
}


	
